#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import os

import rospy

from scan_process.srv import descartes
from scan_process.srv import *


import numpy as np
import scipy as sp
import matplotlib.pyplot as plt
from scipy.optimize import leastsq


# Xi=np.array([0,25,50,75,100])#,43,87,130,173])
# Yi=np.array([0,25,50,75,100])#,75,50,25,0])


##需要拟合的函数func :指定函数的形状 k= 0.42116973935 b= -8.28830260655
def func(p,x):
    k,b=p
    return k*x+b

##偏差函数：x,y都是列表:这里的x,y更上面的Xi,Yi中是一一对应的
def error(p,x,y):
    return func(p,x)-y


class OLS(object):

    def ols(self,req):
        Xi=np.array(req.x)
        Yi=np.array(req.y)

        print "xi"
        print Xi

        print "yi"
        print Yi

        #k,b的初始值，可以任意设定,经过几次试验，发现p0的值会影响cost的值：Para[1]
        p0=[1,20]

        #把error函数中除了p0以外的参数打包到args中(使用要求)
        Para=leastsq(error,p0,args=(Xi,Yi))

        #读取结果
        k,b=Para[0]
        #print("k=",k,"b=",b)

        return descartesResponse(True,k,b)


    def __init__(self):
        rospy.init_node('ols', anonymous=True)
        s = rospy.Service('ols',descartes,self.ols)
        rospy.spin()


def main():
    ols_=OLS()

if __name__ == '__main__':
    main()
